/*
 * GL_Object.h
 *
 * Created 5/31/2009 By Johnny Huynh
 *
 * Version 00.00.05 7/5/2009
 *
 * Copyright Information:
 * All content copyright © 2009 Johnny Huynh. All rights reserved.
 */
 
 /**
  * GL_Object<TYPENAME> is an abstract class for other objects.
  */
 
 // GL_Object.h contains all the object functions
 
 #ifndef GL_OBJECT_H
 #define GL_OBJECT_H
 
 /**
  * GL_OBJECT_COLLISION_DETECTION_TYPE can be defined as 0 or 1.
  * 0 - collision detection, which maintains the entire OBBTree every time the object (i.e. GL_Object) rotates or
  *     change position.
  * 1 - usually more optimal collision detection, which only maintains each OBBTree node up to once per round of 
  *     checking collisions. The OBBTree node is only maintained if the OBBTree node needs to be checked for a 
  *     collision, the object has moved since we last checked for collisions, and the OBBTree node has not 
  *     correspondingly been maintained yet.
  *
  * 0 - maintenance of the OBBTree is optimized for objects with low numbers of polygons compared
  *     to the number of objects in the system. The OBBTree for a object is maintained each time the object
  *     changes its position or orientation. 
  * 1 - additionally optimized for objects of relatively high polygons compared to the number of objects in the system,
  *     maintain only when we are checking for collisions between two OBBTrees and maintain only the two OBBs 
  *     on the current OBBTree depth level that we are checking for collision. Thus, 1 is optimized for all combinations 
  *     of low, high polygons and low, high number of objects in the system.
  */
 #ifndef GL_OBJECT_COLLISION_DETECTION_TYPE
 #define GL_OBJECT_COLLISION_DETECTION_TYPE 1
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE
 
 /**
  * OBBTREE_MAX_DEPTH specifies the maximum depth of the OBBTree
  * Use -1 for a limitless depth for the OBBTree
  */
 #ifndef OBBTREE_MAX_DEPTH
 #define OBBTREE_MAX_DEPTH -1
 #endif // OBBTREE_MAX_DEPTH
 
 /**
  * If IMPLEMENT_INTERPOLATION is set to 1, then the objects will be interpolated (linearly) back to their positions
  * when the objects first collide when collisions occur; otherwise, the objects are not interpolated.
  */
 #ifndef IMPLEMENT_INTERPOLATION
 #define IMPLEMENT_INTERPOLATION 1
 #endif // IMPLEMENT_INTERPOLATION
 
 /**
  * Specifies whether to interpolate gradually (i.e. over time) or not
  */
 #ifndef INTERPOLATE_GRADUALLY
 #define INTERPOLATE_GRADUALLY 0
 #endif // INTERPOLATE_GRADUALLY
 
 /*
  * Specifies whether to readjust the pivot point of the raw object to (0, 0, 0)
  * Set equal to 1 to readjust the pivot point; otherwise, the pivot point will
  * not be readjusted.
  */
 #ifndef READJUST_PIVOT
 #define READJUST_PIVOT 1
 #endif // READJUST_PIVOT
 
 template< typename TYPENAME > class GL_Object;
 
 typedef GL_Object<GLfloat> GL_Objectf;
 typedef GL_Object<GLdouble> GL_Objectd;
 
 #include <math.h>
 #include <vector>
 
 #include "GL_Triangle.h"
 #include "OBBTree.h"
 
 #include "OpenGL_Headers.h"
 #include "GL_Actor.h"
 #include "OrientationMatrix.h"
 #include "CollisionInfo.h"
 
 #include "GL_Render_Shape.h"

 template< typename TYPENAME >
 class GL_Object : public GL_Actor<TYPENAME>
 {
 // Friend Classes
    friend class OBBTree< TYPENAME >;
 
 // Data Members
 protected:
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
    bool isMaintained;
    bool hasMoved;  // this should be changed to a function using position != previousPosition
                    // or, may want to keep hasMoved to keep track of change in rotation
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 #if IMPLEMENT_INTERPOLATION == 1
    Vector3<TYPENAME> previousPosition;
 #endif // IMPLEMENT_INTERPOLATION == 1
 #if INTERPOLATE_GRADUALLY == 1
    bool isInterpolating;
 #endif // INTERPOLATE_GRADUALLY == 1
    std::vector< GL_Triangle<TYPENAME> > raw_triangles; // the positions of the raw triangles needs to be relative to the 
	                                                    // object at position (0, 0, 0) and orientation( 1.0f, 0.0f, 0.0f,
	                                                    //                                               0.0f, 1.0f, 0.0f,
	                                                    //                                               0.0f, 0.0f, 1.0f )
	std::vector< GL_Triangle<TYPENAME> > triangles;
    OBBTree< TYPENAME > obbtree;
    
 // Local Functions
 public:
    GL_Object( std::vector< GL_Triangle<TYPENAME> > raw_triangles,
                               const TYPENAME& posX = 0.0f, const TYPENAME& posY = 0.0f, const TYPENAME& posZ = 0.0f,
                               const TYPENAME& Xx = 1.0f, const TYPENAME& Yx = 0.0f, const TYPENAME& Zx = 0.0f,
                               const TYPENAME& Xy = 0.0f,  const TYPENAME& Yy = 1.0f, const TYPENAME& Zy = 0.0f,
                               const TYPENAME& Xz = 0.0f,  const TYPENAME& Yz = 0.0f, const TYPENAME& Zz = 1.0f );
                               
    GL_Object( const GL_Triangle<TYPENAME>* const& raw_triangles_Ptr, size_t size,
                               const TYPENAME& posX = 0.0f, const TYPENAME& posY = 0.0f, const TYPENAME& posZ = 0.0f,
                               const TYPENAME& Xx = 1.0f, const TYPENAME& Yx = 0.0f, const TYPENAME& Zx = 0.0f,
                               const TYPENAME& Xy = 0.0f,  const TYPENAME& Yy = 1.0f, const TYPENAME& Zy = 0.0f,
                               const TYPENAME& Xz = 0.0f,  const TYPENAME& Yz = 0.0f, const TYPENAME& Zz = 1.0f );
    GL_Object( const GL_Object<TYPENAME>& obj );
    virtual ~GL_Object();
    virtual inline GL_Object<TYPENAME>& operator=( const GL_Object<TYPENAME>& obj );
    inline const std::vector< const GL_Triangle<TYPENAME> >& getTriangles() const;
    virtual inline void updateDisplacement( const TYPENAME& unit ) = 0;
    virtual inline void handleCollision() = 0;
    
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 0
    inline void operator=( const GL_Actor<TYPENAME>& a ){ GL_Actor<TYPENAME>::operator=( a ); maintain( *this ); }
    inline void setPosition( const TYPENAME x, const TYPENAME y, const TYPENAME z )
                                                { GL_Actor<TYPENAME>::setPosition( x, y, z ); maintain( *this ); }
    inline void setOrientation( const TYPENAME Xx, const TYPENAME Yx, const TYPENAME Zx,
                                const TYPENAME Xy, const TYPENAME Yy, const TYPENAME Zy,
                                const TYPENAME Xz, const TYPENAME Yz, const TYPENAME Zz ) 
                                                { GL_Actor<TYPENAME>::setOrientation( Xx, Yx, Zx,
                                                                                      Xy, Yy, Zy,
                                                                                      Xz, Yz, Zz );
                                                  maintain( *this ); }
    inline void setOrientation( const OrientationMatrix3<TYPENAME>& orientation ) {
                                                                    GL_Actor<TYPENAME>::setOrientation( orientation );
                                                                    maintain( *this );}           
    inline void translate( const TYPENAME x, const TYPENAME y, const TYPENAME z ) 
                                                { GL_Actor<TYPENAME>::translate( x, y, z ); maintain( *this ); }
    inline void translate( const Vector3<TYPENAME>& displacement )
                                                { GL_Actor<TYPENAME>::translate( displacement ); maintain( *this ); }
    inline void moveForward( const TYPENAME unit ){ GL_Actor<TYPENAME>::moveForward( unit ); maintain( *this ); }
    inline void moveLeft( const TYPENAME unit ){ GL_Actor<TYPENAME>::moveLeft( unit ); maintain( *this ); }
    virtual inline void globalRotateLeft( const TYPENAME radian )
                                                { GL_Actor<TYPENAME>::globalRotateLeft( radian ); maintain( *this ); }
    virtual inline void globalRotateUp( const TYPENAME radian )
                                                { GL_Actor<TYPENAME>::globalRotateUp( radian ); maintain( *this ); }
    virtual inline void globalTiltRight( const TYPENAME radian )
                                                { GL_Actor<TYPENAME>::globalTiltRight( radian ); maintain( *this ); }
    inline void localRotateLeft( const TYPENAME radian ){ GL_Actor<TYPENAME>::localRotateLeft( radian ); maintain( *this ); }
    inline void localRotateUp( const TYPENAME radian ){ GL_Actor<TYPENAME>::localRotateUp( radian ); maintain( *this ); }
    inline void localTiltRight( const TYPENAME radian ){ GL_Actor<TYPENAME>::localTiltRight( radian ); maintain( *this ); }
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 0
 
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
    inline void operator=( const GL_Actor<TYPENAME>& a )
       { hasMoved = true; GL_Actor<TYPENAME>::operator=( a ); isMaintained = false; }
    inline void setPosition( const TYPENAME x, const TYPENAME y, const TYPENAME z )
       { hasMoved = true; GL_Actor<TYPENAME>::setPosition( x, y, z ); isMaintained = false; }
    inline void setOrientation( const TYPENAME Xx, const TYPENAME Yx, const TYPENAME Zx,
                                const TYPENAME Xy, const TYPENAME Yy, const TYPENAME Zy,
                                const TYPENAME Xz, const TYPENAME Yz, const TYPENAME Zz ) { hasMoved = true;
                                                                           GL_Actor<TYPENAME>::setOrientation( Xx, Yx, Zx,
                                                                                                               Xy, Yy, Zy,
                                                                                                               Xz, Yz, Zz );
                                                                           isMaintained = false; }
    inline void setOrientation( const OrientationMatrix3<TYPENAME>& orientation ) { hasMoved = true;
                                                                    GL_Actor<TYPENAME>::setOrientation( orientation );
                                                                    isMaintained = false; }
    inline void translate( const TYPENAME x, const TYPENAME y, const TYPENAME z )
       { hasMoved = true; GL_Actor<TYPENAME>::translate( x, y, z ); isMaintained = false; }
    inline void translate( const Vector3<TYPENAME>& displacement )
       { hasMoved = true; GL_Actor<TYPENAME>::translate( displacement ); isMaintained = false; }
    inline void moveForward( const TYPENAME unit )
       { hasMoved = true; GL_Actor<TYPENAME>::moveForward( unit ); isMaintained = false; }
    inline void moveLeft( const TYPENAME unit )
       { hasMoved = true; GL_Actor<TYPENAME>::moveLeft( unit ); isMaintained = false; }
    virtual inline void globalRotateLeft( const TYPENAME radian )
       { hasMoved = true; GL_Actor<TYPENAME>::globalRotateLeft( radian ); isMaintained = false; }
    virtual inline void globalRotateUp( const TYPENAME radian )
       { hasMoved = true; GL_Actor<TYPENAME>::globalRotateUp( radian ); isMaintained = false; }
    virtual inline void globalTiltRight( const TYPENAME radian )
       { hasMoved = true; GL_Actor<TYPENAME>::globalTiltRight( radian ); isMaintained = false; }
    inline void localRotateLeft( const TYPENAME radian )
       { hasMoved = true; GL_Actor<TYPENAME>::localRotateLeft( radian ); isMaintained = false; }
    inline void localRotateUp( const TYPENAME radian )
       { hasMoved = true; GL_Actor<TYPENAME>::localRotateUp( radian ); isMaintained = false; }
    inline void localTiltRight( const TYPENAME radian )
       { hasMoved = true; GL_Actor<TYPENAME>::localTiltRight( radian ); isMaintained = false; }
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 
 #if IMPLEMENT_INTERPOLATION == 1
    inline void setPreviousPosition( const Vector3<TYPENAME>& location ) { previousPosition = location; };
 #endif // IMPLEMENT_INTERPOLATION == 1
 
 // Private Functions
 private:
    inline void initializeOBBTree();
    inline const GL_Triangle<TYPENAME>& getTriangle( long index ) const;
    inline size_t getTriangleIndex( const GL_Triangle<TYPENAME>& triangle ) const;
 #if IMPLEMENT_INTERPOLATION == 1
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 0
    inline bool hasMoved() const
    {
        return position != previousPosition;
    }
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 0
 #endif // IMPLEMENT_INTERPOLATION == 1
 
 // Friend Functions
 public:
    template <typename TYPENAME> friend inline void adjust_pivot( GL_Object<TYPENAME>& obj );
    template <typename TYPENAME> friend inline Vector3<TYPENAME> raw_center( const GL_Object<TYPENAME>& obj );
    template <typename TYPENAME> friend inline Vector3<TYPENAME> center( const GL_Object<TYPENAME>& obj );
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 0
    template <typename TYPENAME> friend inline bool collide( const GL_Object<TYPENAME>& objA, 
                                                             const GL_Object<TYPENAME>& objB );
 #if IMPLEMENT_INTERPOLATION == 1
    template <typename TYPENAME> friend inline void checkCollision_Type0( GL_Object<TYPENAME>& obj, 
                                                                    GL_Object<TYPENAME>** objects, const size_t size );
 #endif // IMPLEMENT_INTERPOLATION == 1
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 0
    template <typename TYPENAME> friend inline void checkCollision( GL_Object<TYPENAME>& obj, 
                                                                    GL_Object<TYPENAME>** objects, const size_t size );
    template <typename TYPENAME> friend inline void maintain( GL_Object<TYPENAME>& obj );
    //template <typename TYPENAME> friend inline void print( const GL_Object<TYPENAME>& obj );
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 0
    template <typename TYPENAME> friend inline void render( const GL_Object<TYPENAME>& obj, const GLint& depth = 0x0 );
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 0
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
    template <typename TYPENAME> friend inline void render( GL_Object<TYPENAME>& obj, const GLint& depth = 0x0 );
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 };

 /** LOCAL FUNCTIONS **/
 
 /**
  * Constructor
  * The constructor takes in a vector of triangles.
  * The triangles are expected to approximately surround the position of the object 
  * such that the position of the object is approximately the centroid of the object 
  * for correct collision interpolation; albeit, the best approach is to use the 
  * normal of the colliding triangles.
  *
  * @param (std::vector< GL_Triangle<TYPENAME> >) raw_triangles
  */
 template< typename TYPENAME >
 GL_Object<TYPENAME>::GL_Object( std::vector< GL_Triangle<TYPENAME> > raw_triangles, 
                                 const TYPENAME& posX, const TYPENAME& posY, const TYPENAME& posZ,
                                 const TYPENAME& Xx, const TYPENAME& Yx, const TYPENAME& Zx,
                                 const TYPENAME& Xy, const TYPENAME& Yy, const TYPENAME& Zy,
                                 const TYPENAME& Xz, const TYPENAME& Yz, const TYPENAME& Zz )
                                : GL_Actor<TYPENAME>( posX, posY, posZ,
                                                        Xx, Yx, Zx,
                                                        Xy, Yy, Zy,
                                                        Xz, Yz, Zz ),
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
                                  isMaintained( true ),
                                  hasMoved( false ),
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 #if IMPLEMENT_INTERPOLATION == 1
                                  previousPosition( posX, posY, posZ ),
 #if INTERPOLATE_GRADUALLY == 1
                                  isInterpolating( false ),
 #endif // INTERPOLATE_GRADUALLY == 1
 #endif // IMPLEMENT_INTERPOLATION == 1
                                  raw_triangles( raw_triangles ), triangles( raw_triangles ),
                                  obbtree( this, OBBTREE_MAX_DEPTH )
 {
 /*std::vector< const GL_Triangle<TYPENAME>* > raw_triangle_Addresses;
 size_t numberOfTriangles = raw_triangles.size();
 for ( size_t i = 0x0; i < numberOfTriangles; ++i )
 {
    raw_triangle_Addresses.push_back( &raw_triangles[i] );
 }
 ConvexHull<TYPENAME> ch( &raw_triangle_Addresses[0],raw_triangle_Addresses.size() );
 this->raw_triangles.assign( ch.triangles.begin(), ch.triangles.end() );
 this->triangles.assign( ch.triangles.begin(), ch.triangles.end() );*/
 
 #if READJUST_PIVOT == 1
    adjust_pivot( *this );
 #endif // READJUST_PIVOT
 
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 0
    // if the orientation or position of the object is not representative of the raw orientation and position
    if ( !orientation.isIdentityMatrix() || !position.isZeroVector() )
        maintain( *this );  // recompute the triangles' orientation and positions
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 0  
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
    // if the orientation or position of the object is not representative of the raw orientation and position
    if ( !orientation.isIdentityMatrix() || !position.isZeroVector() )
    {
        isMaintained = false;
        maintain( *this );  // recompute the triangles' orientation and positions
    }
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 
    initializeOBBTree();
 }
 
 /**
  * Alternative Constructor
  * The constructor takes in triangles pointer and the number of triangles.
  * The positions of the raw triangles are assumed to approximately surround the centroid at (0, 0, 0);
  * albeit, the best approach is to use the normal of the colliding triangles.
  *
  * @param (const GL_Triangle<TYPENAME>* const&) raw_triangles_Ptr - this can be the beginning address of an array
  * @param (size_t) size - the number of triangles point to by triangles_Ptr
  */
 template< typename TYPENAME >
 GL_Object<TYPENAME>::GL_Object( const GL_Triangle<TYPENAME>* const& raw_triangles_Ptr, size_t size, 
                                 const TYPENAME& posX, const TYPENAME& posY, const TYPENAME& posZ,
                                 const TYPENAME& Xx, const TYPENAME& Yx, const TYPENAME& Zx,
                                 const TYPENAME& Xy, const TYPENAME& Yy, const TYPENAME& Zy,
                                 const TYPENAME& Xz, const TYPENAME& Yz, const TYPENAME& Zz )
                                : GL_Actor<TYPENAME>( locX, locY, locZ,
                                                      Xx, Yx, Zx,
                                                      Xy, Yy, Zy,
                                                      Xz, Yz, Zz ),
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
                                  isMaintained( true ),
                                  hasMoved( false ),
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 #if IMPLEMENT_INTERPOLATION == 1
                                  previousPosition( posX, posY, posZ ),
 #if INTERPOLATE_GRADUALLY == 1
                                  isInterpolating( false ),
 #endif // INTERPOLATE_GRADUALLY == 1
 #endif // IMPLEMENT_INTERPOLATION == 1
                                  raw_triangles( raw_triangles_Ptr, raw_triangles_Ptr + size),
                                  triangles( raw_triangles_Ptr, raw_triangles_Ptr + size),
                                  obbtree( *this, OBBTREE_MAX_DEPTH )
 {
    // notice that triangles_Ptr + 1 points to the next triangle.
 
 #if READJUST_PIVOT == 1
    adjust_pivot( *this );
 #endif // READJUST_PIVOT
 
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 0
    // if the orientation or position of the object is not representative of the raw orientation and position
    if ( !orientation.isIdentityMatrix() || !position.isZeroVector() )
        maintain( *this );  // recompute the triangles' orientation and positions
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 0  
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
    // if the orientation or position of the object is not representative of the raw orientation and position
    if ( !orientation.isIdentityMatrix() || !position.isZeroVector() )
    {
        isMaintained = false;
        maintain( *this );  // recompute the triangles' orientation and positions
    }
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
    
    initializeOBBTree();
 }
 
 /**
  * Copy Constructor
  */
 template <typename TYPENAME>
 GL_Object<TYPENAME>::GL_Object( const GL_Object<TYPENAME>& obj )
                                 : GL_Actor<TYPENAME>( obj ), 
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
                                   isMaintained( obj.isMaintained ),
                                   hasMoved( obj.hasMoved ), 
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 #if IMPLEMENT_INTERPOLATION == 1
                                   previousPosition( obj.position ),
 #if INTERPOLATE_GRADUALLY == 1
                                  isInterpolating( obj.isInterpolating ),
 #endif // INTERPOLATE_GRADUALLY == 1
 #endif // IMPLEMENT_INTERPOLATION == 1
                                   raw_triangles( obj.raw_triangles ), 
                                   triangles( obj.triangles ), obbtree( obj.obbtree, this, OBBTREE_MAX_DEPTH )
 {
    // copy obbtree
    //obbtree = obj.obbtree;
 }
 
 /**
  * Destructor
  */
 template< typename TYPENAME >
 GL_Object<TYPENAME>::~GL_Object()
 {
 
 }
 
 /**
  * operator=() copies the content of the specified object to this object.
  *
  * @param (const GL_Object<TYPENAME>&) o
  */
 template< typename TYPENAME >
 inline GL_Object<TYPENAME>& GL_Object<TYPENAME>::operator=( const GL_Object<TYPENAME>& obj )
 {
    memcpy( this, &obj, sizeof( GL_Actor<TYPENAME> ) 
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
                + sizeof( isMaintained ) 
                + sizeof( hasMoved )
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 #if IMPLEMENT_INTERPOLATION == 1
                + sizeof( previousPosition )
 #if INTERPOLATE_GRADUALLY == 1
                + sizeof( isInterpolating )
 #endif // INTERPOLATE_GRADUALLY == 1
 #endif // IMPLEMENT_INTERPOLATION == 1
                );
    
    // copy raw_triangles
    this->raw_triangles.clear();
    this->raw_triangles.insert( this->raw_triangles.begin(), obj.raw_triangles.begin(), obj.raw_triangles.end() );
    
    // copy triangles
    this->triangles.clear();
    this->triangles.insert( this->triangles.begin(), obj.triangles.begin(), obj.triangles.end() );
    
    // copy obbtree
    obbtree = obj.obbtree;
    
    return *this;
 }
 
 /**
  * getTriangles() returns a list of triangles that constitute this object.
  *
  * @return const std::vector< const GL_Triangle<TYPENAME> >&
  */
 template< typename TYPENAME >
 inline const std::vector< const GL_Triangle<TYPENAME> >& GL_Object<TYPENAME>::getTriangles() const
 {
    return triangles;
 }
 
 /** PRIVATE FUNCTIONS **/
 
 /**
  * initializeOBBTree() initializes the obbtree for this object.
  */
 template< typename TYPENAME >
 inline void GL_Object<TYPENAME>::initializeOBBTree()
 {
    /*std::vector< const GL_Triangle<TYPENAME>* > raw_triangle_Addresses;
    std::vector< const GL_Triangle<TYPENAME>* > triangle_Addresses;
    size_t numberOfTriangles = raw_triangles.size();
    for ( size_t i = 0x0; i < numberOfTriangles; ++i )
    {
        raw_triangle_Addresses.push_back( &raw_triangles[i] );
        triangle_Addresses.push_back( &triangles[i] );
    }
    obbtree = OBBTree<TYPENAME>( this, OBBTREE_MAX_DEPTH, raw_triangle_Addresses, triangle_Addresses );*/
    if ( raw_triangles.size() > 0 )
        obbtree = OBBTree<TYPENAME>( this, OBBTREE_MAX_DEPTH, &raw_triangles[0], &triangles[0], raw_triangles.size() );
 }
 
 /**
  * getTriangle() returns the triangle of the specified index in the list of triangles
  * that makes up this object.
  *
  * @param (long) index
  * @return const GL_Triangle<TYPENAME>&
  */
 template< typename TYPENAME >
 inline const GL_Triangle<TYPENAME>& GL_Object<TYPENAME>::getTriangle( long index ) const
 {
    return triangles[ index ];
 }
 
 /**
  * getTriangleIndex() returns the index of the specified triangle in the list contained by this object.
  *
  * @param (const GL_Triangle<TYPENAME>&) triangle
  * @return size_t
  */
 template< typename TYPENAME >
 inline size_t GL_Object<TYPENAME>::getTriangleIndex( const GL_Triangle<TYPENAME>& triangle ) const
 {
    /*printf("triangle: %1d\n", ( triangles.size() > 0 ? &triangles[0] : 0 ) );
    printf("triangles: %1d\n", &triangles );
    printf("iterator: %1d\n", triangles.begin() );*/
    /*printf("triangle: %1d\n", &triangle);
    printf("triangle[0]: %1d\n", &triangles[0]);
    printf("triangle difference: %1d\n", &triangle - &triangles[0]);
    printf("triangleSize: %1d\n", sizeof( GL_Triangle<TYPENAME> ));
    printf("triangleIndex: %1d\n", (&triangle - &triangles[0]) / sizeof( GL_Triangle<TYPENAME> ) );*/
    /*printf("triangleIndex: %1d\n", &triangle );
    printf("trianglesIndex0: %1d\n", &triangles[0] );
    printf("triangleSubIndex: %1d\n", (&triangle - &triangles[0]) );*/
    return ( triangles.size() > 0 ? (&triangle - &triangles[0]) /*/ sizeof( GL_Triangle<TYPENAME> )*/ : -1 );
 }
 
 /** FRIEND FUNCTIONS **/
 
 /**
  * adjust_pivot() moves the raw_triangles of the object to have a pivot at Vector3<TYPENAME>( 0, 0, 0 ).
  * The triangles of the object are also readjusted relative to the raw_triangles. The pivot point of the object
  * would then be at the center of the object.
  *
  * @param (GL_Object<TYPENAME>&) obj
  */ 
 template <typename TYPENAME> 
 inline void adjust_pivot( GL_Object<TYPENAME>& obj )
 {
    size_t triangle_index( obj.raw_triangles.size() );
    if ( triangle_index > 0 )
    {
        Vector3<TYPENAME> negative_centroid( -raw_center( obj ) );
        
        if ( !negative_centroid.isZeroVector() )
            do
            {
                --triangle_index;
                
                // translate the triangle in the opposite direction of the centroid vector
                // so that the triangle is situated as if the centroid is at (0, 0, 0)
                apply( obj.raw_triangles[ triangle_index ], negative_centroid );
                apply( obj.triangles[ triangle_index ], negative_centroid );
                
            } while ( triangle_index > 0 );
    }
 }
 
 /**
  * raw_center() returns the center of the raw_triangles of the specified object.
  *
  * @param (const GL_Object<TYPENAME>&) obj
  * @return Vector3<TYPENAME>
  */ 
 template <typename TYPENAME> 
 inline Vector3<TYPENAME> raw_center( const GL_Object<TYPENAME>& obj )
 {
    std::vector< Vector3<TYPENAME> > uniqueVertices;
    
    if ( obj.raw_triangles.size() > 0 )
    {
        getUniqueVertices( uniqueVertices, &obj.raw_triangles[0], obj.raw_triangles.size() );
        
        size_t vertex_index( uniqueVertices.size() - 1 );
        
        // if there is more than 1 vertex in uniqueVertices
        if ( vertex_index > 0 )
        {
            Vector3<TYPENAME> vertex( uniqueVertices[ vertex_index ] );
            TYPENAME minX( vertex.x );
            TYPENAME maxX( minX );
            TYPENAME minY( vertex.y );
            TYPENAME maxY( minY );
            TYPENAME minZ( vertex.z );
            TYPENAME maxZ( minZ );
            
            do
            {
                --vertex_index;
                
                vertex = uniqueVertices[ vertex_index ];
                
                if ( vertex.x < minX )
                    minX = vertex.x;
                else if ( vertex.x > maxX )
                    maxX = vertex.x;
                
                if ( vertex.y < minY )
                    minY = vertex.y;
                else if ( vertex.y > maxY )
                    maxY = vertex.y;
                
                if ( vertex.z < minZ )
                    minZ = vertex.z;
                else if ( vertex.z > maxZ )
                    maxZ = vertex.z;
                
            } while ( vertex_index > 0 );
            
            return Vector3<TYPENAME>( (maxX + minX) / 2.0f, (maxY + minY) / 2.0f, (maxZ + minZ) / 2.0f );
        }
        
        return uniqueVertices[ vertex_index ];
    }
    
    return Vector3<TYPENAME>( ZERO, ZERO, ZERO );
 }
 
 /**
  * center() returns the center of the specified object.
  *
  * @param (const GL_Object<TYPENAME>&) obj
  * @return Vector3<TYPENAME>
  */ 
 template <typename TYPENAME> 
 inline Vector3<TYPENAME> center( const GL_Object<TYPENAME>& obj )
 {
    std::vector< Vector3<TYPENAME> > uniqueVertices;
    
    if ( obj.triangles.size() > 0 )
    {
        getUniqueVertices( uniqueVertices, &obj.triangles[0], obj.triangles.size() );
        
        size_t vertex_index( uniqueVertices.size() - 1 );
        
        // if there is more than 1 vertex in uniqueVertices
        if ( vertex_index > 0 )
        {
            Vector3<TYPENAME> vertex( uniqueVertices[ vertex_index ] );
            TYPENAME minX( vertex.x );
            TYPENAME maxX( minX );
            TYPENAME minY( vertex.y );
            TYPENAME maxY( minY );
            TYPENAME minZ( vertex.z );
            TYPENAME maxZ( minZ );
            
            do
            {
                --vertex_index;
                
                vertex = uniqueVertices[ vertex_index ];
                
                if ( vertex.x < minX )
                    minX = vertex.x;
                else if ( vertex.x > maxX )
                    maxX = vertex.x;
                
                if ( vertex.y < minY )
                    minY = vertex.y;
                else if ( vertex.y > maxY )
                    maxY = vertex.y;
                
                if ( vertex.z < minZ )
                    minZ = vertex.z;
                else if ( vertex.z > maxZ )
                    maxZ = vertex.z;
                
            } while ( vertex_index > 0 );
            
            return Vector3<TYPENAME>( (maxX + minX) / 2.0f, (maxY + minY) / 2.0f, (maxZ + minZ) / 2.0f );
        }
        
        return uniqueVertices[ vertex_index ];
    }
    
    return Vector3<TYPENAME>( ZERO, ZERO, ZERO );
 }
 
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 0
 /**
  * collide() returns true if the two specified objects intersect; otherwise, false is returned.
  *
  * @param (const GL_Object<TYPENAME>&) objA
  * @param (const GL_Object<TYPENAME>&) objB
  * @return bool
  */
 template <typename TYPENAME>
 inline bool collide( const GL_Object<TYPENAME>& objA, const GL_Object<TYPENAME>& objB )
 {
    return collide( objA.obbtree, objB.obbtree, OBBTREE_MAX_DEPTH );
    
    /*
    size_t numberOfTrianglesForA = objA.triangles.size();
    size_t numberOfTrianglesForB = objB.triangles.size();
    for ( size_t i = 0x0; i < numberOfTrianglesForA; ++i )  // for each triangle in objA
    {
        const GL_Triangle<TYPENAME>& triangleA = objA.triangles[i];//A_triangles[i];
        
        for ( size_t j = 0x0; j < numberOfTrianglesForB; ++j ) // for each triangle in objB
        {
            const GL_Triangle<TYPENAME>& triangleB = objB.triangles[j];//B_triangles[j];
            printf("{");
            print( triangleA );
            print( triangleB );
            printf("}");
            if ( collide( triangleA, triangleB ) )
            {
                return true;
            }
        }
    }
    return false;*/
 }
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 0
 
 /**
  * checkCollision() detects and handles the collision between the specified object and objects in the array
  * of object pointers.
  *
  * @param (GL_Object<TYPENAME>&) obj - the object used to detect and handle the collision
  * @param (GL_Object<TYPENAME>**) objects - an array of object pointers
  * @param (const size_t) size - the number of objects in the array of object pointers
  */
 template <typename TYPENAME> 
 inline void checkCollision( GL_Object<TYPENAME>& obj, GL_Object<TYPENAME>** objects, const size_t size )
 {
 
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 0
    // Check for collisions
 #if IMPLEMENT_INTERPOLATION != 1
    for ( size_t i( 0 ); i < size; ++i )
    {
        if ( collide( obj, *(objects[i]) ) )
        {
            obj.handleCollision();
            objects[i]->handleCollision();
        }
    }
 #endif // IMPLEMENT_INTERPOLATION != 1
 #if IMPLEMENT_INTERPOLATION == 1
    checkCollision_Type0( obj, objects, size );
 #endif // IMPLEMENT_INTERPOLATION == 1
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 0
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
    
    // Notice we do not check for collisions between two objects that have not moved.
    // We only check for collisions between two objects if at least one of the object has moved
    // since we last checked for a collision.
    if ( obj.hasMoved )
    {
        maintain( obj );
        
        for ( size_t i( 0 ); i < size; ++i )
        {
            if ( objects[i]->hasMoved )
                maintain( *objects[i] );
 
 #if IMPLEMENT_INTERPOLATION == 1
 //#if 0
            bool collisionOccurred( false );
            //Vector3<TYPENAME> displacementA( obj.position - obj.previousPosition );
            //Vector3<TYPENAME> displacementB( objects[i]->position - objects[i]->previousPosition );
            
            CollisionInfo<TYPENAME> c_info( computeCollisionInfo( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH, 
                                 obj.position - obj.previousPosition, objects[i]->position - objects[i]->previousPosition ) );
 
 //#define DEBUG_GL_OBJECT     
 #ifdef DEBUG_GL_OBJECT
            CollisionInfo<TYPENAME> c_test_info( collideE( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH ) );
            size_t stall_num(0);
 #endif // DEBUG_GL_OBJECT
            
            // while-loop is necessary because colliding polygons are only interpolated out of the polygons they are
            // currently colliding with. After interpolation, polygons may collide with other polygons.
            while ( c_info.overlapping_extent > 0.0f )
            {
                collisionOccurred = true;
  #ifdef DEBUG_GL_OBJECT
                ++stall_num;//printf("Stuck1 - collision_extent: %1f\n", c_info.overlapping_extent);
  //#if 0
                if ( stall_num > 10 )
                {
                    printf("freeze\n");
                }
                
                if ( c_test_info.overlapping_extent > EPSILON && c_info.overlapping_extent < -EPSILON ) //collide( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH ) && c_info.overlapping_extent < -EPSILON )
                {
                        // Triangle Test
                        bool e = collide( c_test_info.triangleA, c_test_info.triangleB );
                        Vector3<TYPENAME> displacementA( Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ) );
                        Vector3<TYPENAME> displacementB( objects[i]->position - objects[i]->previousPosition );
                        TYPENAME f = collisionExtent( c_test_info.triangleA, c_test_info.triangleB, displacementA, displacementB );
                        bool e2 = collide( c_info.triangleA, c_info.triangleB ); 
                        TYPENAME f2 = collisionExtent( c_info.triangleA, c_info.triangleB, displacementA, displacementB );
                        /*
                        // OBB Test
                        bool e = collide( c_test_info.obbA, c_test_info.obbB );
                        Vector3<TYPENAME> displacementA( Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ) );
                        Vector3<TYPENAME> displacementB( objects[i]->position - objects[i]->previousPosition );
                        TYPENAME f = collisionExtent( c_test_info.obbA, c_test_info.obbB, displacementA, displacementB );
                        bool e2 = collide( c_info.obbA, c_info.obbB ); 
                        TYPENAME f2 = collisionExtent( c_info.obbA, c_info.obbB, displacementA, displacementB );
                        */
                        printf("test\n");
                }
 //#endif // #if 0
 //#if 0
                if ( c_test_info.overlapping_extent < EPSILON && c_info.overlapping_extent > EPSILON )
                {
                    // Triangle Test
                    bool e = collide( c_test_info.triangleA, c_test_info.triangleB );
                    Vector3<TYPENAME> displacementA( Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ) );
                    Vector3<TYPENAME> displacementB( objects[i]->position - objects[i]->previousPosition );
                    TYPENAME f = collisionExtent( c_test_info.triangleA, c_test_info.triangleB, displacementA, displacementB );
                    bool e2 = collide( c_info.triangleA, c_info.triangleB ); 
                    TYPENAME f2 = collisionExtent( c_info.triangleA, c_info.triangleB, displacementA, displacementB );
                    /*
                    // OBB Test
                    bool e = collide( c_test_info.obbA, c_test_info.obbB );
                    Vector3<TYPENAME> displacementA( Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ) );
                    Vector3<TYPENAME> displacementB( objects[i]->position - objects[i]->previousPosition );
                    TYPENAME f = collisionExtent( c_test_info.obbA, c_test_info.obbB, displacementA, displacementB );
                    bool e2 = collide( c_info.obbA, c_info.obbB ); 
                    TYPENAME f2 = collisionExtent( c_info.obbA, c_info.obbB, displacementA, displacementB );
                    */
                    printf("Wrong\n");
                }
                //printf("displacementA: " );
                //print( c_info.displacementA );
                //printf("displacementB: " );
                //print( c_info.displacementB );
 //#endif // #if 0
 #endif // DEBUG_GL_OBJECT
                //Vector3<TYPENAME> netVelocity( displacementA - displacementB );
                //Vector3<TYPENAME> netVelocity_Magnitude( magnitude( netVelocity );
                TYPENAME reverse_displacement_extent( c_info.overlapping_extent + EPSILON );
 #ifdef DEBUG_GL_OBJECT
                if ( reverse_displacement_extent > c_info.overlapping_extent + 0.0001f )
                {
                    printf("problem\n");
                }
                
                if ( !c_info.displacementA.isZeroVector() )//if ( (collisionOccurred && hasFiredHitBox) && !c_info.displacementA.isZeroVector() )
                {
                   printf("problem\n");
                }
 #endif // DEBUG_GL_OBJECT
                if ( !c_info.displacementA.isZeroVector() )
                {
                    obj.previousPosition = obj.position;
                    obj.translate( reverse_displacement_extent * -c_info.displacementA );
                    maintain( obj );
                    //obj.previousPosition = obj.position; // set previousPosition to prevent multiple collision issues
                    
                    // temp_displacement
                    Vector3<TYPENAME> temp_dp( obj.position - obj.previousPosition );
                    temp_dp.normalize();
                    /*if ( (temp_dp.x > -EPSILON || temp_dp.x < EPSILON) && (temp_dp.y > -EPSILON || temp_dp.y < EPSILON)
                            && (temp_dp.z > -EPSILON || temp_dp.z < EPSILON) )
                    {
                        temp_dp *= EPSILON_INVERSE;
                    }*/
                    obj.previousPosition = obj.position + temp_dp;//obj.position - obj.previousPosition;
                    
                    c_info.displacementA = -temp_dp; //obj.position - obj.previousPosition; //obj.previousPosition - obj.position;
                    //c_info.displacementA = obj.previousPosition - obj.position;
                    //c_info.displacementA.x = 0.0f;
                    //c_info.displacementA.y = 0.0f;
                    //c_info.displacementA.z = 0.0f;
                
                    //obj.previousPosition = obj.position;
                    //obj.translate( reverse_displacement_extent * -c_info.displacementA );
                    //maintain( obj );
                    //obj.previousPosition = obj.position; // set previousPosition to prevent multiple collision issues
                    
                    //c_info.displacementA = obj.previousPosition - obj.position;
                    //c_info.displacementA.x = 0.0f;
                    //c_info.displacementA.y = 0.0f;
                    //c_info.displacementA.z = 0.0f;
                
                }
                if ( !c_info.displacementB.isZeroVector() )
                {
                    objects[i]->previousPosition = objects[i]->position;
                    objects[i]->translate( reverse_displacement_extent * -c_info.displacementB );
                    maintain( *objects[i] );
                    //objects[i]->previousPosition = objects[i]->position; // set previousPosition to prevent multiple collision issues
                    
                    // temp_displacement
                    Vector3<TYPENAME> temp_dp( objects[i]->position - objects[i]->previousPosition );
                    temp_dp.normalize();
                    //temp_dp *= EPSILON_INVERSE;
                    /*if ( (temp_dp.x > -EPSILON || temp_dp.x < EPSILON) && (temp_dp.y > -EPSILON || temp_dp.y < EPSILON)
                            && (temp_dp.z > -EPSILON || temp_dp.z < EPSILON) )
                    {
                        temp_dp *= EPSILON_INVERSE;
                    }*/
                    objects[i]->previousPosition = objects[i]->position + temp_dp;//objects[i]->position - objects[i]->previousPosition;
                    
                    c_info.displacementB = -temp_dp; //objects[i]->position - objects[i]->previousPosition; //objects[i]->previousPosition - objects[i]->position;
                    //c_info.displacementB.x = 0.0f;
                    //c_info.displacementB.y = 0.0f;
                    //c_info.displacementB.z = 0.0f;
                    
                    /*if ( (c_info.displacementB.x > -EPSILON || c_info.displacementB.x < EPSILON) 
                            && (c_info.displacementB.y > -EPSILON || c_info.displacementB.y < EPSILON)
                            && (c_info.displacementB.z > -EPSILON || c_info.displacementB.z < EPSILON) )
                    {
                        c_info.displacementB *= EPSILON_INVERSE;
                    }*/
                }
                
 #if INTERPOLATE_GRADUALLY == 1
                // The reason this is necessary is because of reverse_displacement_extent.
                // When collision_extent or magnitude( displacementA - displacementB ) is very small,
                // the calculation produces an incorrect result (i.e. floating-point error) for 
                // reverse_displacement_extent, thus, even after one interpolation, the two objects 
                // may still remain in collision.
                if ( c_info.overlapping_extent < EPSILON3 )
                    break;
 #endif // INTERPOLATE_GRADUALLY == 1 
                
                c_info = computeCollisionInfo( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH, 
                                                                                 c_info.displacementA, c_info.displacementB );
            }
 
 #ifdef DEBUG_GL_OBJECT
            printf("stall_num1: %1d\n", stall_num );
 #endif // DEBUG_GL_OBJECT
       
            if ( collisionOccurred )
            {
                obj.handleCollision();
                objects[i]->handleCollision();
            }
            
 //#endif // #if 0
 #if 0
            bool collisionOccurred( false );
            Vector3<TYPENAME> displacementA( obj.position - obj.previousPosition );
            Vector3<TYPENAME> displacementB( objects[i]->position - objects[i]->previousPosition );
            
            // Incase the two objects are colliding, but both have the same velocity, it would be 
            // impossible to return to a state where the two objects are not colliding.
            if ( displacementA == displacementB )
            {
                displacementA.x = 0.0f;
                displacementA.y = 0.0f;
                displacementA.z = 0.0f;
            }
            
            if ( displacementA.isZeroVector() && displacementB.isZeroVector() )
            {
                //printf("Both zero vectors1\n");
                // The best approach is to use the normal of the colliding triangles.
                // Be sure to differentiate between single and double-faced triangles.
                // displacementA = -normal( triangleB )
                // displacementB = -normal( triangleA )
                displacementA = objects[i]->position - obj.position;
                displacementB = -displacementA;
            }
            
            TYPENAME collision_extent( collisionExtent( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH, 
                                                                                          displacementA, displacementB ) );
            
            // while-loop is necessary because colliding polygons are only interpolated out of the polygons they are
            // currently colliding with. After interpolation, polygons may collide with other polygons.
            while ( collision_extent >= 0.0f )
            {//printf("Stuck1 - collision_extent: %1f\n", collision_extent);
                collisionOccurred = true;
                //Vector3<TYPENAME> netVelocity( displacementA - displacementB );
                //Vector3<TYPENAME> netVelocity_Magnitude( magnitude( netVelocity );
                TYPENAME reverse_displacement_extent( collision_extent + EPSILON );
                if ( !displacementA.isZeroVector() )
                {
                    obj.translate( reverse_displacement_extent * -displacementA );
                    maintain( obj );
                    obj.previousPosition = obj.position; // set previousPosition to prevent multiple collision issues
                    displacementA = obj.position - obj.previousPosition;
                }
                // We assume hasMoved means displacementB is not the zero vector
                if ( objects[i]->hasMoved )
                {
                    objects[i]->translate( reverse_displacement_extent * -displacementB );
                    maintain( *objects[i] );
                    objects[i]->previousPosition = objects[i]->position; // set previousPosition to prevent multiple collision issues
                    displacementB = objects[i]->position - objects[i]->previousPosition;
                }
                
                // Incase the two objects are colliding, but both have the same velocity, it would be 
                // impossible to return to a state where the two objects are not colliding.
                if ( displacementA == displacementB )
                {
                    displacementA.x = 0.0f;
                    displacementA.y = 0.0f;
                    displacementA.z = 0.0f;
                }
                
                if ( displacementA.isZeroVector() && displacementB.isZeroVector() )
                {
                    //printf("Both zero vectors1\n");
                    // The best approach is to use the normal of the colliding triangles.
                    // Be sure to differentiate between single and double-faced triangles.
                    // displacementA = -normal( triangleB )
                    // displacementB = -normal( triangleA )
                    displacementA = objects[i]->position - obj.position;
                    displacementB = -displacementA;
                }
                
 #if INTERPOLATE_GRADUALLY == 1
                // The reason this is necessary is because of reverse_displacement_extent.
                // When collision_extent or magnitude( displacementA - displacementB ) is very small,
                // the calculation produces an incorrect result (i.e. floating-point error) for 
                // reverse_displacement_extent, thus, even after one interpolation, the two objects 
                // may still remain in collision.
                if ( collision_extent < EPSILON3 )
                    break;
 #endif INTERPOLATE_GRADUALLY == 1
                
                collision_extent = collisionExtent( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH, 
                                                                                      displacementA, displacementB );
            }
            
            if ( collisionOccurred )
            {
                obj.handleCollision();
                objects[i]->handleCollision();
            }
 #endif // #if 0
 #endif // IMPLEMENT_INTERPOLATION == 1
 #if IMPLEMENT_INTERPOLATION != 1         
            if ( collide( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH ) )
            {
                obj.handleCollision();
                objects[i]->handleCollision();
            }
 #endif // IMPLEMENT_INTERPOLATION != 1 
        }
        
        obj.hasMoved = false; // reset hasMoved
    }
    else // obj.hasMoved == false
    {
        for ( size_t i( 0 ); i < size; ++i )
        {
            if ( objects[i]->hasMoved )
            {
                maintain( *objects[i] );
                
 #if IMPLEMENT_INTERPOLATION == 1
 //#if 0
                // Virtually the same as above when using CollisionInfo
                bool collisionOccurred( false );
                //Vector3<TYPENAME> displacementA( Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ) );
                //Vector3<TYPENAME> displacementB( objects[i]->position - objects[i]->previousPosition );
                
                CollisionInfo<TYPENAME> c_info( computeCollisionInfo( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH, 
                               Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ), objects[i]->position - objects[i]->previousPosition ) );
 
 #ifdef DEBUG_GL_OBJECT
                CollisionInfo<TYPENAME> c_test_info( collideE( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH ) );
                
                //size_t counter( 5 );
                size_t stall_num(0);
                TYPENAME previous_reverse_extent( 0.0f );
 #endif // DEBUG_GL_OBJECT
                
                // while-loop is necessary because colliding polygons are only interpolated out of the polygons they are
                // currently colliding with. After interpolation, polygons may collide with other polygons.
                while ( c_info.overlapping_extent > 0.0f )
                {
                    collisionOccurred = true;
 #ifdef DEBUG_GL_OBJECT
                    ++stall_num;//
                    printf("Stuck2 - collision_extent: %1f\n", c_info.overlapping_extent);
                    printf("c_info.displacementB: ");
                    print( c_info.displacementB );
                    /*if ( magnitude( c_info.displacementB ) > 5 )
                    {
                        Vector3<TYPENAME> disA( Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ) );
                        Vector3<TYPENAME> disB( objects[i]->position - objects[i]->previousPosition );
                        TYPENAME extent_test( collisionExtent( c_info.triangleA, c_info.triangleB, disA, disB ) );
                        CollisionInfo<TYPENAME> c_info_test( computeCollisionInfo( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH, 
                               Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ), objects[i]->position - objects[i]->previousPosition ) );
                    }*/
                    /*printf("Stuck2 - obj1: %1d obj2: %2d\n", &obj, objects[i]);
                    printf("Begin: objects[i]->position\n");
                    print(objects[i]->position);
                    printf("Begin: objects[i]->previousPosition\n");
                    print(objects[i]->previousPosition);*/
                    
                    //if ( !collide( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH ) && c_info.overlapping_extent > EPSILON )
  //#if 0
                    if ( c_test_info.overlapping_extent > EPSILON && c_info.overlapping_extent < -EPSILON ) //collide( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH ) && c_info.overlapping_extent < -EPSILON )
                    {
                            // Triangle Test
                            bool e = collide( c_test_info.triangleA, c_test_info.triangleB );
                            Vector3<TYPENAME> displacementA( Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ) );
                            Vector3<TYPENAME> displacementB( objects[i]->position - objects[i]->previousPosition );
                            TYPENAME f = collisionExtent( c_test_info.triangleA, c_test_info.triangleB, displacementA, displacementB );
                            bool e2 = collide( c_info.triangleA, c_info.triangleB ); 
                            TYPENAME f2 = collisionExtent( c_info.triangleA, c_info.triangleB, displacementA, displacementB );
                            /*
                            // OBB Test
                            bool e = collide( c_test_info.obbA, c_test_info.obbB );
                            Vector3<TYPENAME> displacementA( Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ) );
                            Vector3<TYPENAME> displacementB( objects[i]->position - objects[i]->previousPosition );
                            TYPENAME f = collisionExtent( c_test_info.obbA, c_test_info.obbB, displacementA, displacementB );
                            bool e2 = collide( c_info.obbA, c_info.obbB ); 
                            TYPENAME f2 = collisionExtent( c_info.obbA, c_info.obbB, displacementA, displacementB );
                            */
                            printf("test\n");
                    }
 //#endif // #if 0
 //#if 0
                    if ( c_test_info.overlapping_extent < EPSILON && c_info.overlapping_extent > EPSILON )
                    {
                        // Triangle Test
                        bool e = collide( c_test_info.triangleA, c_test_info.triangleB );
                        Vector3<TYPENAME> displacementA( Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ) );
                        Vector3<TYPENAME> displacementB( objects[i]->position - objects[i]->previousPosition );
                        TYPENAME f = collisionExtent( c_test_info.triangleA, c_test_info.triangleB, displacementA, displacementB );
                        bool e2 = collide( c_info.triangleA, c_info.triangleB ); 
                        TYPENAME f2 = collisionExtent( c_info.triangleA, c_info.triangleB, displacementA, displacementB );
                        /*
                        // OBB Test
                        bool e = collide( c_test_info.obbA, c_test_info.obbB );
                        Vector3<TYPENAME> displacementA( Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ) );
                        Vector3<TYPENAME> displacementB( objects[i]->position - objects[i]->previousPosition );
                        TYPENAME f = collisionExtent( c_test_info.obbA, c_test_info.obbB, displacementA, displacementB );
                        bool e2 = collide( c_info.obbA, c_info.obbB ); 
                        TYPENAME f2 = collisionExtent( c_info.obbA, c_info.obbB, displacementA, displacementB );
                        */
                        printf("Wrong\n");
                    }
                    /*printf("displacementA: " );
                    print( c_info.displacementA );
                    printf("displacementB: " );
                    print( c_info.displacementB );*/
 //#endif // #if 0
 #endif // DEBUG_GL_OBJECT
                    //Vector3<TYPENAME> netVelocity( displacementA - displacementB );
                    //Vector3<TYPENAME> netVelocity_Magnitude( magnitude( netVelocity );
                    TYPENAME reverse_displacement_extent( c_info.overlapping_extent + EPSILON );
 
 #ifdef DEBUG_GL_OBJECT
                    //printf("reverse_displacement_extent: %1f\n", reverse_displacement_extent);
                    if ( !c_info.displacementA.isZeroVector() )
                    {
                        printf("problem\n");
                    }
                    
                    if ( reverse_displacement_extent > c_info.overlapping_extent + 0.0001f )
                    {
                        printf("problem\n");
                    }
 // Current Debug
                    if ( stall_num > 100 )
                    {
                        printf("problem\n");
                        // OBB Test
                        bool e = collide( c_test_info.obbA, c_test_info.obbB );
                        Vector3<TYPENAME> displacementA( Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ) );
                        Vector3<TYPENAME> displacementB( objects[i]->position - objects[i]->previousPosition );
                        TYPENAME f = collisionExtent( c_test_info.obbA, c_test_info.obbB, displacementA, displacementB );
                        bool e2 = collide( c_info.obbA, c_info.obbB ); 
                        TYPENAME f2 = collisionExtent( c_info.obbA, c_info.obbB, displacementA, displacementB );
                    }                  
                    /*if ( (reverse_displacement_extent - previous_reverse_extent > 2.0f && previous_reverse_extent != ZERO)
                        || c_info.overlapping_extent > 1.5f )
                    {
                        // Triangle Test
                        bool e = collide( c_test_info.triangleA, c_test_info.triangleB );
                        Vector3<TYPENAME> displacementA( Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ) );
                        Vector3<TYPENAME> displacementB( objects[i]->position - objects[i]->previousPosition );
                        TYPENAME f = collisionExtent( c_test_info.triangleA, c_test_info.triangleB, displacementA, displacementB );
                        bool e2 = collide( c_info.triangleA, c_info.triangleB ); 
                        TYPENAME f2 = collisionExtent( c_info.triangleA, c_info.triangleB, displacementA, displacementB );
                        printf("problem\n");
                    }*/
                    
                    previous_reverse_extent = reverse_displacement_extent;
 #endif // DEBUG_GL_OBJECT
                                       
                    if ( !c_info.displacementA.isZeroVector() )
                    {
                        //TYPENAME temp_extent( c_info.displacementA * c_info.displacementB );
                        
                        // if obj is immobile
                        
                        // if displacementA and displacementB point away from each other
                        /*if ( temp_extent > -EPSILON3 && temp_extent < EPSILON3 )
                        {printf("Bad\n");
                            objects[i]->translate( reverse_displacement_extent * c_info.displacementA );
                        }
                        else
                        {printf("Good\n");
                            objects[i]->translate( ( reverse_displacement_extent + temp_extent ) * c_info.displacementB );
                        }*/
                        
                        // if obj is mobile
                        //obj.translate( reverse_displacement_extent * -c_info.displacementA );
                        //maintain( obj );
                        //obj.previousPosition = obj.position; // set previousPosition to prevent multiple collision issues
                        
                        //c_info.displacementA = Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f );
                        c_info.displacementA.x = 0.0f;
                        c_info.displacementA.y = 0.0f;
                        c_info.displacementA.z = 0.0f;
                    }
                    if ( !c_info.displacementB.isZeroVector() )
                    {
                        objects[i]->previousPosition = objects[i]->position;
                        //if ( !c_info.displacementA.isZeroVector() )
                        //{printf("Test\n");
                            //objects[i]->translate( ( reverse_displacement_extent + ( c_info.displacementA * c_info.displacementB ) ) * c_info.displacementB );
                        //}
                        objects[i]->translate( reverse_displacement_extent * -c_info.displacementB );
                        maintain( *objects[i] );
                        //objects[i]->previousPosition = objects[i]->position; // set previousPosition to prevent multiple collision issues
                        // temp_displacement
                        Vector3<TYPENAME> temp_dp( objects[i]->position - objects[i]->previousPosition );
                        temp_dp.normalize();
                        /*if ( counter > 1 )
                        {
                            temp_dp *= --counter * EPSILON_INVERSE;
                        }
                        else*/
                            //temp_dp *= EPSILON_INVERSE;
                        /*if ( (temp_dp.x > -EPSILON || temp_dp.x < EPSILON) && (temp_dp.y > -EPSILON || temp_dp.y < EPSILON)
                                && (temp_dp.z > -EPSILON || temp_dp.z < EPSILON) )
                        {
                            temp_dp *= EPSILON_INVERSE;
                        }*/
                        objects[i]->previousPosition = objects[i]->position + temp_dp;//objects[i]->position - objects[i]->previousPosition;
                        c_info.displacementB = -temp_dp; //objects[i]->position - objects[i]->previousPosition; //objects[i]->previousPosition - objects[i]->position;
                        //c_info.displacementB.x = 0.0f;
                        //c_info.displacementB.y = 0.0f;
                        //c_info.displacementB.z = 0.0f;
                    }
                    //c_info.displacementA.x = 0.0f;
                    //c_info.displacementA.y = 0.0f;
                    //c_info.displacementA.z = 0.0f;
                    
 #ifdef DEBUG_GL_OBJECT
                    printf("stall_num2: %1d\n", stall_num );
 #endif // DEBUG_GL_OBJECT
                    
 #if INTERPOLATE_GRADUALLY == 1
                    /*printf("End: objects[i]->position\n");
                    print(objects[i]->position);
                    printf("End: objects[i]->previousPosition\n");
                    print(objects[i]->previousPosition);*/
                    // The reason this is necessary is because of reverse_displacement_extent.
                    // When collision_extent or magnitude( displacementA - displacementB ) is very small,
                    // the calculation produces an incorrect result (i.e. floating-point error) for 
                    // reverse_displacement_extent, thus, even after one interpolation, the two objects 
                    // may still remain in collision.
                    //if ( c_info.overlapping_extent < EPSILON3 )
                        break;
 #endif // INTERPOLATE_GRADUALLY == 1
 
 #ifdef DEBUG_GL_OBJECT
                    Vector3<TYPENAME> dA(c_info.displacementA);
                    Vector3<TYPENAME> dB(c_info.displacementB);
 #endif // DEBUG_GL_OBJECT
                    
                    c_info = computeCollisionInfo( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH,
                                                            c_info.displacementA, c_info.displacementB );
                    
 #ifdef DEBUG_GL_OBJECT
                    if ( c_info.displacementB.isZeroVector() )
                    {
                        TYPENAME e( collisionExtent( c_info.triangleA, c_info.triangleB, dA, dB ) );
                    }
                    c_test_info = collideE( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH );
 #endif // DEBUG_GL_OBJECT
                
                }
                              
                if ( collisionOccurred )
                {
                    obj.handleCollision();
                    objects[i]->handleCollision();
                }
  #if INTERPOLATE_GRADUALLY == 1              
                if ( c_info.overlapping_extent > 0.0f )
                    objects[i]->isInterpolating = true;
  #endif // INTERPOLATE_GRADUALLY == 1
                
  //#endif // #if 0
  #if 0
                bool collisionOccurred( false );
                Vector3<TYPENAME> displacementA( Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ) );
                Vector3<TYPENAME> displacementB( objects[i]->position - objects[i]->previousPosition );
                
                // if ( displacementA.isZeroVector() && displacementB.isZeroVector() )
                if ( displacementB.isZeroVector() )
                {
                    //printf("Both zero vectors2\n");
                    // The best approach is to use the normal of the colliding triangles.
                    // Be sure to differentiate between single and double-faced triangles.
                    // displacementA = -normal( triangleB )
                    // displacementB = -normal( triangleA )
                    //displacementA = objects[i]->position - obj.position;
                    //displacementB = -displacementA;
                    displacementB = obj.position - objects[i]->position;
                }
                
                TYPENAME collision_extent( collisionExtent( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH, 
                                                                                              displacementA, displacementB ) );
                
                // while-loop is necessary because colliding polygons are only interpolated out of the polygons they are
                // currently colliding with. After interpolation, polygons may collide with other polygons.
                while ( collision_extent >= 0.0f )
                {//printf("Stuck2 - collision_extent: %1f\n", collision_extent);
                    collisionOccurred = true;
                    //Vector3<TYPENAME> netVelocity( displacementA - displacementB );
                    //Vector3<TYPENAME> netVelocity_Magnitude( magnitude( netVelocity );
                    TYPENAME reverse_displacement_extent( collision_extent + EPSILON );
                    
                    /*if ( !displacementA.isZeroVector() )
                    {printf("Should not be here\n");
                        obj.translate( reverse_displacement_extent * -displacementA );
                        maintain( obj );
                        obj.previousPosition = obj.position; // set previousPosition to prevent multiple collision issues
                    }*/
                    // We assume hasMoved means displacementB is not the zero vector
                    if ( objects[i]->hasMoved )
                    {
                        objects[i]->translate( reverse_displacement_extent * -displacementB );
                        maintain( *objects[i] );
                        objects[i]->previousPosition = objects[i]->position; // set previousPosition to prevent multiple collision issues
                        displacementB.x = 0.0f;
                        displacementB.y = 0.0f;
                        displacementB.z = 0.0f;
                    }
                    
                    // displacementA.x = 0.0f;
                    // displacementA.y = 0.0f;
                    // displacementA.z = 0.0f;
                    displacementA = obj.position - obj.previousPosition;
                    
                    // if ( displacementA.isZeroVector() && displacementB.isZeroVector() )
                    if ( displacementB.isZeroVector() )
                    {
                        //printf("Both zero vectors2\n");
                        // The best approach is to use the normal of the colliding triangles.
                        // Be sure to differentiate between single and double-faced triangles.
                        // displacementA = -normal( triangleB )
                        // displacementB = -normal( triangleA )
                        //displacementA = objects[i]->position - obj.position;
                        //displacementB = -displacementA;
                        displacementB = obj.position - objects[i]->position;
                    }
                    
 #if INTERPOLATE_GRADUALLY == 1
                    // The reason this is necessary is because of reverse_displacement_extent.
                    // When collision_extent or magnitude( displacementA - displacementB ) is very small,
                    // the calculation produces an incorrect result (i.e. floating-point error) for 
                    // reverse_displacement_extent, thus, even after one interpolation, the two objects 
                    // may still remain in collision.
                    if ( collision_extent < EPSILON3 )
                        break;
 #endif // INTERPOLATE_GRADUALLY == 1
                                     
                    collision_extent = collisionExtent( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH, 
                                                                                          displacementA, displacementB );
                }
                
                if ( collisionOccurred )
                {
                    obj.handleCollision();
                    objects[i]->handleCollision();
                }
 #endif // #if 0
 #endif // IMPLEMENT_INTERPOLATION == 1
 #if IMPLEMENT_INTERPOLATION != 1
                if ( collide( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH ) )
                {
                    obj.handleCollision();
                    objects[i]->handleCollision();
                }
 #endif // IMPLEMENT_INTERPOLATION != 1
            }
        }
    }
 #if IMPLEMENT_INTERPOLATION == 1
 
 #if INTERPOLATE_GRADUALLY == 1
    if ( !obj.isInterpolating )
    {
        //if ( size == 0 )
            //printf("Setting PreviousPosition for object: %1d\n", &obj);
        obj.previousPosition = obj.position;
    }
    else
        obj.isInterpolating = false;
 #endif // INTERPOLATE_GRADUALLY == 1
 #if INTERPOLATE_GRADUALLY != 1
    obj.previousPosition = obj.position;
 #endif // INTERPOLATE_GRADUALLY != 1
 
 #endif // IMPLEMENT_INTERPOLATION == 1
 #if IMPLEMENT_INTERPOLATION != 1
    obj.previousPosition = obj.position; // set previousPosition (right after collision interpolation is done)
 #endif // IMPLEMENT_INTERPOLATION != 1
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 }
 
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 0
 /**
  * checkCollision_Type0() detects and handles the collision between the specified object and objects in the array
  * of object pointers.
  *
  * @param (GL_Object<TYPENAME>&) obj - the object used to detect and handle the collision
  * @param (GL_Object<TYPENAME>**) objects - an array of object pointers
  * @param (const size_t) size - the number of objects in the array of object pointers
  */
 template <typename TYPENAME> 
 inline void checkCollision_Type0( GL_Object<TYPENAME>& obj, GL_Object<TYPENAME>** objects, const size_t size )
 {
    // Notice we do not check for collisions between two objects that have not moved.
    // We only check for collisions between two objects if at least one of the object has moved
    // since we last checked for a collision.
    if ( obj.hasMoved() )
    {
        //maintain( obj );
        
        for ( size_t i( 0 ); i < size; ++i )
        {
            //if ( objects[i]->hasMoved() )
                //maintain( *objects[i] );
 
 #if IMPLEMENT_INTERPOLATION == 1
            
            bool collisionOccurred( false );
            //Vector3<TYPENAME> obj_displacement( obj.position - obj.previousPosition );
            //Vector3<TYPENAME> objectsi_displacementB( objects[i]->position - objects[i]->previousPosition );
            
            CollisionInfo<TYPENAME> c_info( computeCollisionInfo( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH, 
                                 obj.position - obj.previousPosition, objects[i]->position - objects[i]->previousPosition ) );
            
            // while-loop is necessary because colliding polygons are only interpolated out of the polygons they are
            // currently colliding with. After interpolation, polygons may collide with other polygons.
            while ( c_info.overlapping_extent > 0.0f )
            {
                collisionOccurred = true;
                
                //Vector3<TYPENAME> netVelocity( obj_displacement - objectsi_displacementB );
                //Vector3<TYPENAME> netVelocity_Magnitude( magnitude( netVelocity );
                TYPENAME reverse_displacement_extent( c_info.overlapping_extent + EPSILON );
                
                if ( !c_info.displacementA.isZeroVector() )
                {
                    obj.previousPosition = obj.position;
                    obj.translate( reverse_displacement_extent * -c_info.displacementA );
                    maintain( obj );
                    //obj.previousPosition = obj.position; // set previousPosition to prevent multiple collision issues
                    
                    // temp_displacement
                    Vector3<TYPENAME> temp_dp( obj.position - obj.previousPosition );
                    //temp_dp.normalize();
                    
                    obj.previousPosition = obj.position + temp_dp;//obj.position - obj.previousPosition;
                    
                    c_info.displacementA = -temp_dp; //obj.position - obj.previousPosition; //obj.previousPosition - obj.position;
                }
                if ( !c_info.displacementB.isZeroVector() )
                {
                    objects[i]->previousPosition = objects[i]->position;
                    objects[i]->translate( reverse_displacement_extent * -c_info.displacementB );
                    maintain( *objects[i] );
                    //objects[i]->previousPosition = objects[i]->position; // set previousPosition to prevent multiple collision issues
                    
                    // temp_displacement
                    Vector3<TYPENAME> temp_dp( objects[i]->position - objects[i]->previousPosition );
                    temp_dp.normalize();
                    
                    objects[i]->previousPosition = objects[i]->position + temp_dp;//objects[i]->position - objects[i]->previousPosition;
                    
                    c_info.displacementB = -temp_dp; //objects[i]->position - objects[i]->previousPosition; //objects[i]->previousPosition - objects[i]->position;
                }
                
 #if INTERPOLATE_GRADUALLY == 1
                // The reason this is necessary is because of reverse_displacement_extent.
                // When collision_extent or magnitude( displacementA - displacementB ) is very small,
                // the calculation produces an incorrect result (i.e. floating-point error) for 
                // reverse_displacement_extent, thus, even after one interpolation, the two objects 
                // may still remain in collision.
                //if ( c_info.overlapping_extent < EPSILON3 )
                    break;
 #endif // INTERPOLATE_GRADUALLY == 1 
                
                CollisionInfo<TYPENAME> temp_c_info( computeCollisionInfo( obj.obbtree, objects[i]->obbtree, 
                                                        OBBTREE_MAX_DEPTH, c_info.displacementA, c_info.displacementB ) );
                
                if ( temp_c_info.overlapping_extent > 0.0f )
                    c_info = temp_c_info;
                else
                    break;
            }
       
            if ( collisionOccurred )
            {
                Vector3<TYPENAME> previous_obbA_center( c_info.obbA.center );
                Vector3<TYPENAME> previous_obbB_center( c_info.obbB.center );
                maintain( c_info.obbA );
                maintain( c_info.obbB );
                
                obj.handleCollision();
                objects[i]->handleCollision();
                
                if ( global::testCollision )
                {
                glColor3f(1.0f, 0.0f, 0.0f );
                OBB<TYPENAME>* obbA_Ptr( &c_info.obbA );
                const Vector3<TYPENAME> half_dimensionsA( obbA_Ptr->half_width, obbA_Ptr->half_height, obbA_Ptr->half_depth );
 #if OBBTREE_MAX_DEPTH >= 0
                Render_Shape::Box_Solid( obbA_Ptr->center, half_dimensionsA, obbA_Ptr->getOrientation() );
 #endif // OBBTREE_MAX_DEPTH >= 0
 #if OBBTREE_MAX_DEPTH < 0
                render( apply( c_info.triangleA, c_info.obbA.center - previous_obbA_center + (EPSILON2 * normal(c_info.triangleA)) ) );
 #endif // OBBTREE_MAX_DEPTH < 0
                OBB<TYPENAME>* obbB_Ptr( &c_info.obbB );
                glColor3f(0.0f, 1.0f, 0.0f );
                const Vector3<TYPENAME> half_dimensionsB( obbB_Ptr->half_width, obbB_Ptr->half_height, obbB_Ptr->half_depth );
 #if OBBTREE_MAX_DEPTH >= 0
                Render_Shape::Box_Solid( obbB_Ptr->center, half_dimensionsB, obbB_Ptr->getOrientation() );
 #endif // #if OBBTREE_MAX_DEPTH >= 0
 #if OBBTREE_MAX_DEPTH < 0
                render( apply( c_info.triangleB, c_info.obbB.center - previous_obbB_center + (EPSILON2 * normal(c_info.triangleB)) ) );
 #endif // OBBTREE_MAX_DEPTH < 0
                }
            }
 #endif // IMPLEMENT_INTERPOLATION == 1
 #if IMPLEMENT_INTERPOLATION != 1         
            if ( collide( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH ) )
            {
                obj.handleCollision();
                objects[i]->handleCollision();
            }
 #endif // IMPLEMENT_INTERPOLATION != 1 
        }
    }
    else // obj.hasMoved() == false
    {
        for ( size_t i( 0 ); i < size; ++i )
        {
            //if ( objects[i]->hasMoved() )
            //{
                //maintain( *objects[i] );
            //}
 #if IMPLEMENT_INTERPOLATION == 1
            
            bool collisionOccurred( false );
            //Vector3<TYPENAME> obj_displacement( Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ) );
            //Vector3<TYPENAME> objectsi_displacement( objects[i]->position - objects[i]->previousPosition );
            
            CollisionInfo<TYPENAME> c_info( computeCollisionInfo( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH, 
                           Vector3<TYPENAME>( 0.0f, 0.0f, 0.0f ), objects[i]->position - objects[i]->previousPosition ) );
            
            // while-loop is necessary because colliding polygons are only interpolated out of the polygons they are
            // currently colliding with. After interpolation, polygons may collide with other polygons.
            while ( c_info.overlapping_extent > 0.0f )
            {
                collisionOccurred = true;
                
                //Vector3<TYPENAME> netVelocity( obj_displacement - objectsi_displacement );
                //Vector3<TYPENAME> netVelocity_Magnitude( magnitude( netVelocity );
                TYPENAME reverse_displacement_extent( c_info.overlapping_extent + EPSILON );
                // Not sure if possible to get in here, should check
                // Should only be possible if both objects have displacements of zeroes
                if ( !c_info.displacementA.isZeroVector() )
                {
                    c_info.displacementA.x = 0.0f;
                    c_info.displacementA.y = 0.0f;
                    c_info.displacementA.z = 0.0f;
                }
                if ( !c_info.displacementB.isZeroVector() )
                {
                    objects[i]->previousPosition = objects[i]->position;
                    
                    objects[i]->translate( reverse_displacement_extent * -c_info.displacementB );
                    maintain( *objects[i] );
                    
                    // temp_displacement
                    Vector3<TYPENAME> temp_dp( objects[i]->position - objects[i]->previousPosition );
                    //temp_dp.normalize();
                    
                    objects[i]->previousPosition = objects[i]->position + temp_dp;//objects[i]->position - objects[i]->previousPosition;
                    c_info.displacementB = -temp_dp; //objects[i]->position - objects[i]->previousPosition; //objects[i]->previousPosition - objects[i]->position;
                }
                
 #if INTERPOLATE_GRADUALLY == 1
                // The reason this is necessary is because of reverse_displacement_extent.
                // When collision_extent or magnitude( displacementA - displacementB ) is very small,
                // the calculation produces an incorrect result (i.e. floating-point error) for 
                // reverse_displacement_extent, thus, even after one interpolation, the two objects 
                // may still remain in collision.
                //if ( c_info.overlapping_extent < EPSILON3 )
                    break;
 #endif // INTERPOLATE_GRADUALLY == 1
                
                CollisionInfo<TYPENAME> temp_c_info( computeCollisionInfo( obj.obbtree, objects[i]->obbtree, 
                                                        OBBTREE_MAX_DEPTH, c_info.displacementA, c_info.displacementB ) );
                
                if ( temp_c_info.overlapping_extent > 0.0f )
                    c_info = temp_c_info;
                else
                    break;
            }
                          
            if ( collisionOccurred )
            {
                Vector3<TYPENAME> previous_obbB_center( c_info.obbB.center );
                maintain( c_info.obbB );
                
                obj.handleCollision();
                objects[i]->handleCollision();
                
                if ( global::testCollision )
                {
                glColor3f(1.0f, 0.0f, 0.0f );
                const OBB<TYPENAME>* obbA_Ptr( &c_info.obbA );
                const Vector3<TYPENAME> half_dimensionsA( obbA_Ptr->half_width, obbA_Ptr->half_height, obbA_Ptr->half_depth );
 #if OBBTREE_MAX_DEPTH >= 0
                Render_Shape::Box_Solid( obbA_Ptr->center, half_dimensionsA, obbA_Ptr->getOrientation() );
 #endif // OBBTREE_MAX_DEPTH >= 0
 #if OBBTREE_MAX_DEPTH < 0
                render( c_info.triangleA );
 #endif // OBBTREE_MAX_DEPTH < 0
                OBB<TYPENAME>* obbB_Ptr( &c_info.obbB );
                glColor3f(0.0f, 1.0f, 0.0f );
                const Vector3<TYPENAME> half_dimensionsB( obbB_Ptr->half_width, obbB_Ptr->half_height, obbB_Ptr->half_depth );
 #if OBBTREE_MAX_DEPTH >= 0
                Render_Shape::Box_Solid( obbB_Ptr->center, half_dimensionsB, obbB_Ptr->getOrientation() );
 #endif // OBBTREE_MAX_DEPTH >= 0
 #if OBBTREE_MAX_DEPTH < 0
                render( apply( c_info.triangleB, c_info.obbB.center - previous_obbB_center + (EPSILON2 * normal(c_info.triangleB)) ) );
 #endif // OBBTREE_MAX_DEPTH < 0
                }
            }
 #if INTERPOLATE_GRADUALLY == 1              
            if ( c_info.overlapping_extent > 0.0f )
                objects[i]->isInterpolating = true;
 #endif // INTERPOLATE_GRADUALLY == 1
 
 #endif // IMPLEMENT_INTERPOLATION == 1
 #if IMPLEMENT_INTERPOLATION != 1
            if ( collide( obj.obbtree, objects[i]->obbtree, OBBTREE_MAX_DEPTH ) )
            {
                obj.handleCollision();
                objects[i]->handleCollision();
            }
 #endif // IMPLEMENT_INTERPOLATION != 1
        }
    }
 #if IMPLEMENT_INTERPOLATION == 1
 
 #if INTERPOLATE_GRADUALLY == 1
    if ( !obj.isInterpolating )
    {
        //if ( size == 0 )
            //printf("Setting PreviousPosition for object: %1d\n", &obj);
        obj.previousPosition = obj.position;
    }
    else
        obj.isInterpolating = false;
 #endif // INTERPOLATE_GRADUALLY == 1
 #if INTERPOLATE_GRADUALLY != 1
    obj.previousPosition = obj.position;
 #endif // INTERPOLATE_GRADUALLY != 1
 
 #endif // IMPLEMENT_INTERPOLATION == 1
 #if IMPLEMENT_INTERPOLATION != 1
    obj.previousPosition = obj.position; // set previousPosition (right after collision interpolation is done)
 #endif // IMPLEMENT_INTERPOLATION != 1
 }
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 0
 
 /**
  * maintain() recomputes the triangles using the set of raw_triangles, orientation, and
  * position of the specified object. This function also makes a call to maintain the OBBTree.
  *
  * @param (GL_Object<TYPENAME>&) obj
  */
 template <typename TYPENAME>
 inline void maintain( GL_Object<TYPENAME>& obj )
 {
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
    if ( !obj.isMaintained )    // maintain only if the OBBTree has not been maintained yet
    {
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
    
        // maintain the triangles
        
        size_t triangle_index( obj.triangles.size() );
        
        // Copy raw_triangles to triangles
        copyTriangles( &obj.triangles[0], &obj.raw_triangles[0], obj.triangles.size() );
        
        // Apply rotation followed by translation to triangles
        if ( triangle_index > 0 )
            do
            {
                --triangle_index;
                apply( obj.triangles[ triangle_index ], obj.orientation, obj.position );
            } while( triangle_index > 0 );
    
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 0
        // maintain the OBBTree
        maintain( obj.obbtree );
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 0
 
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
        markForMaintenance( obj.obbtree );  
        obj.isMaintained = true; // mark as maintained
    }
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 }
 
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 0
 /**
  * render() contains the code necessary to render an object in OpenGL.
  * The values assigned to depth determines the OBBs on the depth level 
  * in the OBBTree hierarchy that will be rendered.
  * A depth value of < 0 will render the entire OBBTree hierarchy.
  * A depth value of 0 will not render any OBB
  * A depth value of 1 will render the first level in the OBBTree (just 1 OBB)
  * A depth value of n will render 2^(n-1) OBBs for the nth level in the OBBTree
  *
  * @param (const GL_Object<TYPENAME>&) obj
  * @param (const GLint&) depth
  */
 template <typename TYPENAME>
 inline void render( const GL_Object<TYPENAME>& obj, const GLint& depth )
 {
    if ( global::enableConvexHull )
    {
        ConvexHull<TYPENAME> ch( &obj.triangles[0], obj.triangles.size() );
        render( ch );
    }
    else
    {
        size_t numberOfTriangles = obj.triangles.size();
        for ( size_t i( 0x0 ); i < numberOfTriangles; ++i )
        {
            const Triangle<TYPENAME>& tri( obj.triangles[i] );
            Render_Shape::Triangle_Solid( tri.getEndPointA(), tri.getEndPointB(), tri.getEndPointC() );
        }
        
        for ( size_t i( 0x0 ); i < numberOfTriangles; ++i )
        {
            const Triangle<TYPENAME>& tri( obj.triangles[i] );
            glColor3f( 1.0f, 1.0f, 1.0f );
            Render_Shape::Triangle_WireFrame( tri.getEndPointA(), tri.getEndPointB(), tri.getEndPointC() );
        }
    }
    
    render( obj.obbtree, depth );
 }
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 0
 
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 /**
  * render() contains the code necessary to render an object in OpenGL.
  * The values assigned to depth determines the OBBs on the depth level 
  * in the OBBTree hierarchy that will be rendered.
  * A depth value of < 0 will render the entire OBBTree hierarchy.
  * A depth value of 0 will not render any OBB
  * A depth value of 1 will render the first level in the OBBTree (just 1 OBB)
  * A depth value of n will render 2^(n-1) OBBs for the nth level in the OBBTree
  *
  * @param (GL_Object<TYPENAME>&) obj
  * @param (const GLint&) depth
  */
 template <typename TYPENAME>
 inline void render( GL_Object<TYPENAME>& obj, const GLint& depth )
 {
    maintain( obj );
    
    if ( global::enableConvexHull )
    {
        ConvexHull<TYPENAME> ch( &obj.triangles[0], obj.triangles.size() );
        render( ch );
    }
    else
    {
        size_t numberOfTriangles = obj.triangles.size();
        for ( size_t i( 0x0 ); i < numberOfTriangles; ++i )
        {
            const Triangle<TYPENAME>& tri( obj.triangles[i] );
            Render_Shape::Triangle_Solid( tri.getEndPointA(), tri.getEndPointB(), tri.getEndPointC() );
        }
        
        glColor3f( 1.0f, 1.0f, 1.0f );
        
        for ( size_t i( 0x0 ); i < numberOfTriangles; ++i )
        {
            const Triangle<TYPENAME>& tri( obj.triangles[i] );
            Render_Shape::Triangle_WireFrame( tri.getEndPointA(), tri.getEndPointB(), tri.getEndPointC() );
        }
    }
    
    render( obj.obbtree, depth );
 }
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
 
 #endif // GL_OBJECT_H